%% 检查车辆的某个位置是否与障碍物碰撞
function isCollision = VehiclePositionCheck(pos,theta,Vehicle,Configure)
    % 车辆尺寸
    W = Vehicle.W;
    LF = Vehicle.LF;
    LB = Vehicle.LB;

    % 四个角的位置
    Cornerfl = [LF, W/2];
    Cornerfr = [LF, -W/2];
    Cornerrl = [-LB, W/2];
    Cornerrr = [-LB, -W/2];

    % 方向余弦矩阵，负号是 把坐标转换为基坐标
    dcm = angle2dcm(-theta, 0, 0);
    
    % 获取四个角的全局坐标
    tvec = dcm*[Cornerfl';0]; % 旋转变换
    tvec = tvec';
    Cornerfl = tvec(1:2)+pos; % 平移变换
    
    tvec = dcm*[Cornerfr';0];
    tvec = tvec';
    Cornerfr = tvec(1:2)+pos;
    
    tvec = dcm*[Cornerrl';0];
    tvec = tvec';
    Cornerrl = tvec(1:2)+pos;
    
    tvec = dcm*[Cornerrr';0];
    tvec = tvec';
    Cornerrr = tvec(1:2)+pos;

    % 记录构成车辆模型的四条直线的起止坐标
    Rect = [];
    Rect(end+1,:) = [Cornerfl, Cornerfr]; % 前
    Rect(end+1,:) = [Cornerfr, Cornerrr]; % 右
    Rect(end+1,:) = [Cornerrr, Cornerrl]; % 后
    Rect(end+1,:) = [Cornerrl, Cornerfl]; % 左

    % 碰撞检测循环
    ObstList = Configure.OBSTACLE_LIST;
    isCollision = false;
    for i = 1:length(ObstList)
        x = ObstList(i,1);
        y = ObstList(i,2);
        % 检测点(x,y)是否在矩形Rect内
        if inpolygon(x,y,Rect(:,1),Rect(:,2))
            isCollision = true;
            return;
        end
    end
end